
#include "EvolutionOfMorphologyAndBehavior.h"

#include "Creature.h"


#include "BodyPart.h"

#include "ExperimentThread.h"
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_sort.h>
namespace jcrada {

    ExperimentThread::ExperimentThread(EvolutionOfMorphologyAndBehavior& evolution,
            BulletSimulator& simulator) :
    EvolutiveSimulationThread(simulator),
    _experiment(NULL),
    _recorded_creature(NULL) {
        setEvolution(&evolution);
    }

    ExperimentThread::~ExperimentThread() {
        delete _experiment;
    }

    void ExperimentThread::run() {
        if (!getEvolution()) {
            emit message("Evolution not set");
            setStopped(true);
            return;
        }
        int iterations = getIterations();
        emit started();
        QTime timer;
        timer.start();
        int current_iteration = 0;
        for (current_iteration = 0; current_iteration < iterations && !isStopped(); ++current_iteration) {
            iterate();
        }
        getExperiment()._current_iteration = current_iteration;
        getExperiment()._time_in_seconds = timer.elapsed() / 1000;
        analyze();

        setStopped(true);

        emit stopped();
        emit message(QString(("Time Elapsed: " + TO_STRING(timer.elapsed() / 1000.0) + " s.").c_str()));
    }

    void ExperimentThread::stabilize(Creature& creature) {
        btVector3 axis(0.0f, 1.0f, 0.0f);

        btHingeConstraint* constraint = new btHingeConstraint(
                *creature.getRoot().getRigidBody(),
                btVector3(0, 0, 0.0f), axis);
        constraint->setLimit(0.0f, 0.0);
        getSimulator().getWorld().addConstraint(constraint, true);
        bool was_behavior_enabled = isBehaviorEnabled();
        setBehaviorEnabled(false);
        int simulation_steps = getSecondsForStabilization() * getSimulator().getTimestepsPerSecond();
        for (int i = 0; i < simulation_steps; ++i) {
            performStep();
        }
        setBehaviorEnabled(was_behavior_enabled);
        getSimulator().getWorld().removeConstraint(constraint);
        delete constraint;
    }

    void ExperimentThread::iterate() {
        int simulation_steps = getEvolution()->getEvaluationTime() *
                getSimulator().getTimestepsPerSecond();

        double best_fitness = -1.0 / 0.0;
        Creature* best_creature = NULL;
        Creature* creature = NULL;
        double fitness = 0.0;
        double mean_fitness = 0.0;
        getEvolution()->prepareCreatures();
        for (int i = 0; i < getEvolution()->getPopulationSize(); ++i) {
            creature = &getEvolution()->getCreature(i);
            getSimulator().reset();
            getSimulator().addCreature(*creature);
            stabilize(*creature);
            for (int j = 0; j < simulation_steps; ++j) {
                getTimer().restart();
                performStep();
                ensureFrameRate(getTimer().elapsed());
            }
            getSimulator().removeCreature();
            fitness = (creature->getFinalPosition() - creature->getInitialPosition()).length();
            creature->setFitness(fitness);
            if (fitness > best_fitness) {
                best_creature = creature;
                best_fitness = fitness;
            }
            mean_fitness += fitness;
        }
        mean_fitness /= getEvolution()->getPopulationSize();
        Experiment& experiment = getExperiment();
        experiment._fitness_in_time[getCurrentIteration()] = mean_fitness;

        if (getCurrentIteration() < experiment._iterations / experiment._record_best_creatures_modulo) {
            if (_recorded_creature == NULL) {
                double worst_fitness = 1.0 / 0.0;
                for (int i = 0; i < getEvolution()->getPopulationSize(); ++i) {
                    if (getEvolution()->getCreature(i).getFitness() < worst_fitness) {
                        _recorded_creature = &getEvolution()->getCreature(i);
                        worst_fitness = _recorded_creature->getFitness();
                    }
                }
            }
//            int index = getCurrentIteration() / experiment._record_best_creatures_modulo;
            int index = getCurrentIteration();
            experiment._best_creatures[index] = _recorded_creature->clone();
        }

        if (getCurrentIteration() + 1 >= getIterations()) {
            experiment._best_creature_morphology = getEvolution()->getMorphology(
                    getEvolution()->getMorphologyIndex(*best_creature)).clone();
            best_fitness = -1.0;
            for (int i = 0; i < getEvolution()->getNumberOfMorphologies(); ++i) {
                if (getEvolution()->getMorphology(i).getFitness() > best_fitness) {
                    best_fitness = getEvolution()->getMorphology(i).getFitness();
                    experiment._preferred_morphology = &getEvolution()->getMorphology(i);
                }
            }
            experiment._preferred_morphology = experiment._preferred_morphology->clone();
            for (int i = 0; i < experiment._population_size; ++i) {
                experiment._morphology_index[i] = getEvolution()->getMorphologyIndex(i);
            }
        }


        std::string msg = TO_STRING(getCurrentIteration()) + "\t" + TO_STRING(mean_fitness);
        emit message(QString(msg.c_str()));

        getEvolution()->step();
        setCurrentIteration(getCurrentIteration() + 1);
    }

    void ExperimentThread::analyze() {
        getExperiment()._body_parts = getEvolution()->getBodyParts();
        getExperiment()._current_iteration = getCurrentIteration();

        getExperiment()._hidden_layers = getEvolution()->getHiddenLayers();
        getExperiment()._max_mass = getEvolution()->getMaxMass();
        getExperiment()._max_size = getEvolution()->getMaxSize();
        getExperiment()._min_mass = getEvolution()->getMinMass();
        getExperiment()._min_size = getEvolution()->getMinSize();
        getExperiment()._morphologies = getEvolution()->getNumberOfMorphologies();
        getExperiment()._neurons_per_layer = getEvolution()->getNeuronsPerLayer();
        getExperiment()._population_size = getEvolution()->getPopulationSize();
        getExperiment()._evolved_creatures = getEvolution()->getCreatures();
        getExperiment().analyze();
        getExperiment()._pso = &getEvolution()->getPsoModel();
    }

    int ExperimentThread::main(int argc, char** argv) {
        int size = 20;
        double* arr = new double[size];
        for (int i = 0; i < size; ++i) {
            arr[i] = i;
        }
        BDEBUG(TO_STRING(gsl_stats_min(arr, 1, 10)));
    }
}
